#include <stdio.h>
#include "system.h"
#include "io.h"

int flash_get_mid(int base)
{
    int ret;
    IOWR_16DIRECT(base, 0xaaa, 0xaa);
    IOWR_16DIRECT(base, 0x555, 0x55);
    IOWR_16DIRECT(base, 0xaaa, 0x90);
    ret = IORD_16DIRECT(base, 2);
    IOWR_16DIRECT(base, 0, 0xf0);
    return ret;
}

int flash_erase_sector(int base,int sector)
{
    int ret;
    
    if(sector >= 0x010000)
        sector = sector & 0xffff0000;
    else
        sector = sector & 0xffffe000;
    IOWR_16DIRECT(base, 0xaaa, 0xaa);
    IOWR_16DIRECT(base, 0x555, 0x55);
    IOWR_16DIRECT(base, 0xaaa, 0x80);
    IOWR_16DIRECT(base, 0xaaa, 0xaa);
    IOWR_16DIRECT(base, 0x555, 0x55);
    IOWR_16DIRECT(base, sector, 0x30);
    do
    {
        ret=IORD_16DIRECT(base, sector);
    }while((ret & 0xffff) != 0xffff);
    return 0;
}

int flash_write_word(int base, int addr, int data)
{
    int ret;
    
    IOWR_16DIRECT(base, 0xaaa, 0xaa);
    IOWR_16DIRECT(base, 0x555, 0x55);
    IOWR_16DIRECT(base, 0xaaa, 0xa0);
    IOWR_16DIRECT(base, addr, data);
    do
    {
        ret = IORD_16DIRECT(base, addr);        
    }while(ret != data);
    return 0;
}

int flash_write_file(int base, int addr, int *data, int bcnt)
{
    int ret;
    int writedata;
    int i;
    int scaddr;
    
    printf("Erasing sector [0x%x] ", addr);
    if(addr >= 0x010000)
        scaddr = addr & 0xffff0000;
    else
        scaddr = addr & 0xffffe000;
    
    do
    {
        ret = flash_erase_sector(base, scaddr);
        printf(".");
        if(scaddr >= 0x00010000)
            scaddr = scaddr + 0x00010000;
        else
            scaddr = scaddr + 0x00001000;
    } while(scaddr < (addr + bcnt));
    printf(" done\nProgramming sector [0x%x]", addr);
    for(i = 0; i < bcnt; i += 4)
    {
        writedata = data[i>>2] & 0x0000ffff;
        ret = flash_write_word(base, addr+i, writedata);
        writedata = (data[i>>2] & 0xffff0000) >> 16;
        ret = flash_write_word(base, addr + i + 2, writedata);
        printf(".");
    }
    printf(" done\n[%d bytes writed]\n", bcnt);
    return 0;
}
